#include "calcdistance.h"
#include "object_distance.h"

using namespace cv;

CalcDistance::CalcDistance(QObject *parent)
	: QThread(parent),
	poseCorrect(nullptr),
	distance(nullptr)
{
}

CalcDistance::~CalcDistance()
{
}

void CalcDistance::setImage(cv::Mat& image)
{
	this->image = image.clone();
}

void CalcDistance::setCameraParameter(const QString& distanceFilename, const QString& calibFilename)
{
	FileStorage fs(distanceFilename.toStdString(), FileStorage::READ);

	fs["focal_length"] >> cameraParameter.focalLength;
	fs["x_cell_size"] >> cameraParameter.xCellSize;
	fs["y_cell_size"] >> cameraParameter.yCellSize;
	fs["object_radius"] >> parameters["object_radius"];
	fs["square_length"] >> parameters["square_length"];
	fs["scale"] >> parameters["distance_scale"];
	fs["reference"] >> parameters["distance_reference"];

	fs.release();

	fs.open(calibFilename.toStdString(), FileStorage::READ);

	fs["camera_matrix"] >> cameraParameter.cameraMatrix;
	fs["dist_coeffs"] >> cameraParameter.distCoeffs;

	fs.release();
}

void CalcDistance::setPoseCorrect(PoseCorrect* poseCorrect)
{
	this->poseCorrect = poseCorrect;
}

void CalcDistance::setDistance(Distance* distance)
{
	this->distance = distance;
}


void CalcDistance::run()
{
	if (image.empty() || cameraParameter.focalLength == 0 || distance == nullptr) {
		emit processed(-1);
		return;
	}

	Mat correctImage = image;
	if (poseCorrect != nullptr) {
		poseCorrect->correct(image, correctImage);
	}

	distance->setCameraParameter(cameraParameter, parameters);
	double length = distance->calculate(correctImage);

	emit processed(length);
}

